#include "../../include/vision/VisioneController.h"
#include <libfreenect.hpp>
#include <iostream>
#include <vector>
#include <cmath>
#include <math.h>
#include <pthread.h>
#include <opencv/cv.h>
#include <opencv2/opencv.hpp>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>
using namespace cv;
using namespace std;

VisioneController::VisioneController(){

	rgbMatTmp = Mat(Size(640,480),CV_8UC3,Scalar(0));
	rgbMat = Mat(Size(640,240),CV_8UC3,Scalar(0));
	depthMatTmp = Mat(Size(640,480),CV_16UC1);
	depthMat = Mat(Size(640,240),CV_16UC1);
	hsvMat = new Mat(Size(640,240),CV_8UC3,Scalar(0));
	infPalla=Scalar(0,175,22);
	supPalla=Scalar(8,255,255);
	img_palla_filt2 = Mat(Size(640,240),CV_8UC1,Scalar(0));
	/*MATTINA:*/ infPorta=Scalar(103,143,38);
	/*MATTINA:*/ supPorta=Scalar(141,182,110);
	/*ORE 13:*/  //infPorta=Scalar(87,143,20);
	/*ORE 13:*/  //supPorta=Scalar(180,182,155);
	/*ORE 15:*/  //infPorta=Scalar(88,112,20);
	/*ORE 15:*/  //supPorta=Scalar(117,109,215);
	filtro= new Filtro();

	img_porta_filt2 = Mat(Size(640,240),CV_8UC1,Scalar(0));
	porta= new RiconoscimentoPorta();
	palla= new RiconoscimentoPalla();
	img = cvCreateImage(cvSize(640, 240), IPL_DEPTH_8U, 3);

	device = &freenect.createDevice<MyFreenectDevice>(0);
	device->startVideo();
	device->startDepth();
	device->setDepthFormat(FREENECT_DEPTH_MM);
}

VisioneController::~VisioneController(){
	device->stopDepth();
	device->stopVideo();
}
void VisioneController::elaborateImage(){
	device->getVideo(rgbMatTmp);
	device->getDepth(depthMatTmp);
	rgbMat = rgbMatTmp(cv::Range(240, 480-1), cv::Range(0, 640-1));
	depthMat = depthMatTmp(cv::Range(240, 480-1), cv::Range(0, 640-1));

	cvtColor(rgbMat,*hsvMat,CV_BGR2HSV);
	img_palla_filt2=filtro->FiltroImg(*hsvMat,Scalar(MIN_H_PALLA,MIN_S_PALLA,MIN_V_PALLA), Scalar(SUP_H_PALLA,SUP_S_PALLA,SUP_V_PALLA));
	img_porta_filt2=filtro->FiltroImg(*hsvMat,Scalar(MIN_H_PORTA,MIN_S_PORTA,MIN_V_PORTA), Scalar(SUP_H_PORTA,SUP_S_PORTA,SUP_V_PORTA));

	//img_ostacolo_filt2=filtro->FiltroImg(hsvMat,infOstacolo,supOstacolo);

	palla= new RiconoscimentoPalla();
	centroPalla=palla->riconoscimento_palla(rgbMat,img_palla_filt2);
	profPalla = NULL;
	if(centroPalla.x!=NULL && centroPalla.y!=NULL){
		profPalla=palla->calcoloProfonditaPalla(rgbMat,depthMat,centroPalla);
		angoloPalla=palla->calcoloAngoloPalla(rgbMat,centroPalla);
	}

	*img=hsvMat->operator _IplImage();
	memcpy(img->imageData, hsvMat->data, 640 * 240 * 3 * sizeof(char));
	porta= new RiconoscimentoPorta();
	maxRettangoloPorta=porta->riconoscimento_porta(rgbMat,img_porta_filt2);

	profPorta = NULL;
	if(maxRettangoloPorta.area()!=0){
		cvSetImageROI(img,maxRettangoloPorta);
		rettangoloRoi=cvCreateImage(cvSize(maxRettangoloPorta.width,maxRettangoloPorta.height),img->depth,img->nChannels);
		cvCopy(img,rettangoloRoi,NULL);
		cvResetImageROI(img);
		profPorta=porta->calcoloProfonditaPorta(rgbMat,depthMat,maxRettangoloPorta,rettangoloRoi);
		angoloPorta=porta->calcoloAngoloPorta(rgbMat,Point(maxRettangoloPorta.x+maxRettangoloPorta.width/2,maxRettangoloPorta.y+maxRettangoloPorta.height/2));
	}

	imshow("img_tagliata",rgbMat);

	cvWaitKey(100);
}
unsigned short VisioneController::getProfonditaPorta(){
	return profPorta;
}
int VisioneController::getAngoloPorta(){
	return angoloPorta;
}
unsigned short VisioneController::getProfonditaPalla(){
	return profPalla;
}
int VisioneController::getAngoloPalla(){
	return angoloPalla;
}
void VisioneController::alza(){
	device->setTiltDegrees(5);
}
void VisioneController::abbassa(){
	device->setTiltDegrees(-5);
}
void VisioneController::azzera(){
	device->setTiltDegrees(0);
}
